//
// Created by jwhww on 25-9-23.
//

#include "AllHeadFile.h"

/**
 * 欢迎大家使用JWH的RM电机驱动库！
 * 该库基于RM电机的GM6020、M3508、M2006电机，支持位置、速度、电流（力矩）分别的单环模式，以及位置速度双串环模式，未来或许会支持位置速度力矩三串环模式。
 * 该库的使用方法如下：
 *                  在main.c中初始化底层CAN、定时器、PID控制器：bsp_can_init(), bsp_tim_init(), pid_init()
 *                  ↓
 *                  在main.c中初始化电机，选定要使用的电机类型，可多类型电机：JJ_motor_init(MOTOR_TYPE_GM6020), JJ_motor_init(MOTOR_TYPE_M3508), JJ_motor_init(MOTOR_TYPE_M2006)
 *                  ↓
 *                  在can的Rx中断服务函数 HAL_CAN_RxFifo0MsgPendingCallback 中处理接收到的电机数据：JJ_motor_can_receive(&hcan1);
 *                  ↓
 *                  （开环）在Time6的中断回调函数 HAL_TIM_PeriodElapsedCallback 中直接先装载JJ_motor_data_load后发报JJ_motor_ctrl来控制电机（最大1kHz），并选用Voltage、Current模式，例如：GROUP_1234_CURRENT
 *                 或↓
 *                  （闭环）在Time6的中断回调函数 HAL_TIM_PeriodElapsedCallback 中先运行closeCircle系列闭环函数，再装载JJ_motor_data_load，最后发报JJ_motor_ctrl控制电机（最大1kHz），closeCircle前面写的是啥就是在对啥进行闭环;
 *
 *                  *注意* 多电机的话请全部 closeCircle 完毕并且弄个缓存区保存好,之后一起装载 JJ_motor_data_load，最后一次性发报JJ_motor_ctrl控制电机,详见callback.c文件示例
 *                  *注意* 由于电机返回报文频率固定为1kHz，因此Time6的控制频率最好不大于1kHz，高了没用。
 *                  *设想* 直接在CAN的Rx中断回调中发报文也不是不可以哈哈哈但我没试过你可以试试
 * 该库的代码结构如下：
 * 1. 总头文件：AllHeadFile.h，包含了所有的头文件，请只在源文件中引用。
 * 2. 电机驱动：JJ_rm_motor.c，包含了电机的初始化、数据加载、数据处理、数据发送等函数。
 * 3. 板级驱动：bsp_can.c、bsp_tim.c，包含了CAN总线、定时器的初始化。
 * 4. 通用驱动：pid.c、clip.c，包含了PID控制器、限幅函数。
 * 5. 其他文件：CmakeLists.txt，包含了项目的cmake配置文件，可设置include path以及executable source目标编译源文件。
 * 6. 其他文件：callback.c，包含了所有热门中断回调函数的重定义。
 * 7. 其他文件：dbus.c，遥控器解算相关。
 * 祝大家使用愉快~！
 */

//---------------电机can相关变量初始化-----------------
// Tx相关
CAN_TxHeaderTypeDef TxHeader_can_GM6020; // CAN1发送消息头结构体，一个can总线只需要初始化一个
CAN_TxHeaderTypeDef TxHeader_can_M3508;
CAN_TxHeaderTypeDef TxHeader_can_M2006;
uint32_t Tx_mail_can_1;                     // CAN1发送邮箱号（用于标识发送缓冲区）
uint32_t Tx_mail_can_2;
uint8_t load_status = 0;                    // 装载成功标志位 0-失败 1-成功
uint8_t can_send_content[8] = {0};           // CAN报文发送区
// Rx相关
CAN_RxHeaderTypeDef RxHeader_can_GM6020; // CAN1接收消息头结构体，一个can总线只需要初始化一个
CAN_RxHeaderTypeDef RxHeader_can_M3508;
CAN_RxHeaderTypeDef RxHeader_can_M2006;
uint8_t flag_can_receive = 0;               //收报标志位 0-正在收报 1-收报完成
// 电机相关
MotorInfo Motor_Info[8] = {0};	// 用户电机信息结构体实例，用于存储接收到的电机数据,表示电机当前信息。聚合初始化为0
// MotorActivated activating_motor[8] = {DEACTIVATING};  // （弃用）该电机是否初始化, 0-未使用 1-已使用
//---------------电机can相关变量初始化-----------------

//------------------------------------------------------------------------------------------------------
// 函数简介		  电机相关初始化
// 使用示例         JJ_motor_init(GM6020);
// 参数介绍         counts_of_motor                 输入要初始化的电机个数，只能按顺序初始化,6020是1~7,3508是1~8
// 返回参数         0-正常
// 备注信息         一旦启用电机，只能一次性启用一组4个电机（6020第二组只有3个），JJ_motor_data_load报文发送0即可关闭对应ID的单个电机
//------------------------------------------------------------------------------------------------------
int JJ_motor_init(const MotorType motor_type) {
    //初始化
    switch (motor_type) {

        case MOTOR_TYPE_GM6020:
            TxHeader_can_GM6020.ExtId = 0;
            TxHeader_can_GM6020.IDE = CAN_ID_STD;
            TxHeader_can_GM6020.RTR = CAN_RTR_DATA;
            TxHeader_can_GM6020.DLC = 8;
            TxHeader_can_GM6020.TransmitGlobalTime = DISABLE;
            break;

        case MOTOR_TYPE_M3508:
            TxHeader_can_M3508.ExtId = 0;
            TxHeader_can_M3508.IDE = CAN_ID_STD;
            TxHeader_can_M3508.RTR = CAN_RTR_DATA;
            TxHeader_can_M3508.DLC = 8;
            TxHeader_can_M3508.TransmitGlobalTime = DISABLE;
            break;

        case MOTOR_TYPE_M2006:
            TxHeader_can_M2006.ExtId = 0;
            TxHeader_can_M2006.IDE = CAN_ID_STD;
            TxHeader_can_M2006.RTR = CAN_RTR_DATA;
            TxHeader_can_M2006.DLC = 8;
            TxHeader_can_M2006.TransmitGlobalTime = DISABLE;
            break;

        default:
            break;
    }
    return 0;
}

//------------------------------------------------------------------------------------------------------
// 函数简介       can装载函数
// 使用示例       JJ_motor_can_load(0,0,0,0);
// 参数介绍       motor_id                  要驱动的电机ID
// 参数介绍       value                     豹纹（报文）
// 返回参数		void
// 备注信息       内部调用；
// 备注信息       ！！！如果不想让某个电机运动，请将对应的数据位清零！！！
// 备注信息       电压给定值范围：-25000~0~25000，对应±24V
// 备注信息       电流给定值范围：-16384~0~16384, 对应最大转矩电流范围-3A~0~3A。注意！连续负载最大1.62A（±8852），对应最大连续扭矩1.2N·m，不然会烧
// 备注信息       堵转安全电流0.9A（±4915），检测到堵转之后请手动调整电流到0.9A（±4915）以下
// 备注信息       关于“堵转”和“带载”的定义，请结合需求自己确定
//------------------------------------------------------------------------------------------------------
void JJ_motor_data_load(const MotorID motor_id, const int16_t value) {
    if (motor_id == MOTOR_1 || motor_id == MOTOR_5) {
        //ID为1或5的电机
        can_send_content[0] = (value & 0xff00) >> 8;		//装载拆解过后的高低8位can数据
        can_send_content[1] = (value & 0x00ff);
        load_status = 1;    //成功
    } else if (motor_id == MOTOR_2 || motor_id == MOTOR_6) {
        //ID为2或6的电机
        can_send_content[2] = (value & 0xff00) >> 8;
        can_send_content[3] = (value & 0x00ff);
        load_status = 1;    //成功
    } else if (motor_id == MOTOR_3 || motor_id == MOTOR_7) {
        //ID为3或7电机
        can_send_content[4] = (value & 0xff00) >> 8;
        can_send_content[5] = (value & 0x00ff);
        load_status = 1;    //成功
    }else if (motor_id == MOTOR_4 || motor_id == MOTOR_8) {
        //ID为4或8电机
        can_send_content[6] = (value & 0xff00) >> 8;
        can_send_content[7] = (value & 0x00ff);
        load_status = 1;    //成功
    }
}

//------------------------------------------------------------------------------------------------------
// 函数简介		重置装载篓子
// 使用示例
// 返回参数		void
// 备注信息
//------------------------------------------------------------------------------------------------------
void JJ_motor_data_refresh(void) {
    // 重置发文装载篓子
    memset(can_send_content, 0, sizeof(can_send_content));
}
//------------------------------------------------------------------------------------------------------
// 函数简介		can发送函数
// 使用示例		JJ_motor_ctrl(&hcan1, GM6020, GROUP_1234_CURRENT);
// 参数介绍		*hcan                           can总线结构体，hcan1、hcan2
// 参数介绍       motor_type                      电机类型
// 参数介绍       motor_group                     电机组和控制类型
// 返回参数		void
// 备注信息       需要先装载再发报，并且一次会发送整group的报文；目前只兼容GM6020
// 备注信息       想开环控制可以直接调用这个，别调用那些posion、speed、torque之类的
//------------------------------------------------------------------------------------------------------
int JJ_motor_ctrl(CAN_HandleTypeDef *hcan, MotorType motor_type, MotorGroup motor_group) {
    switch (motor_type) {

        case MOTOR_TYPE_GM6020:
            //发报
            if (hcan == &hcan1) {
                if (motor_group == MOTOR_GROUP_1234_VOLTAGE)
                    TxHeader_can_GM6020.StdId = 0x1FF;      //电压控制
                else if (motor_group == MOTOR_GROUP_1234_CURRENT)
                    TxHeader_can_GM6020.StdId = 0x1FE;      //电流控制
                for (uint8_t i = 0; i < 3; i++) {                   //发信失败则再试2次
                    if (HAL_OK == HAL_CAN_AddTxMessage(hcan,&TxHeader_can_GM6020,can_send_content,&Tx_mail_can_1))
                        break;
                }
                //后四个电机
                if (motor_group == MOTOR_GROUP_5678_VOLTAGE)
                    TxHeader_can_GM6020.StdId = 0x2FF;
                else if (motor_group == MOTOR_GROUP_5678_CURRENT)
                    TxHeader_can_GM6020.StdId = 0x2FE;
                for (uint8_t i = 0; i < 3; i++) {                   //发信失败则再试2次
                    if (HAL_OK == HAL_CAN_AddTxMessage(hcan,&TxHeader_can_GM6020,can_send_content,&Tx_mail_can_1))
                        break;
                }
            }
            else if (hcan == &hcan2) {
                if (motor_group == MOTOR_GROUP_1234_VOLTAGE)
                    TxHeader_can_GM6020.StdId = 0x1FF;      //电压控制
                else if (motor_group == MOTOR_GROUP_1234_CURRENT)
                    TxHeader_can_GM6020.StdId = 0x1FE;      //电流控制
                for (uint8_t i = 0; i < 3; i++) {                   //发信失败则再试2次
                    if (HAL_OK == HAL_CAN_AddTxMessage(hcan,&TxHeader_can_GM6020,can_send_content,&Tx_mail_can_2))
                        break;
                }
                //后四个电机
                if (motor_group == MOTOR_GROUP_5678_VOLTAGE)
                    TxHeader_can_GM6020.StdId = 0x2FF;
                else if (motor_group == MOTOR_GROUP_5678_CURRENT)
                    TxHeader_can_GM6020.StdId = 0x2FE;
                for (uint8_t i = 0; i < 3; i++) {                   //发信失败则再试2次
                    if (HAL_OK == HAL_CAN_AddTxMessage(hcan,&TxHeader_can_GM6020,can_send_content,&Tx_mail_can_2))
                        break;
                }
            }
            else {
                return -1;
            }
            break;

        case MOTOR_TYPE_M3508:
            //发报
            if (hcan == &hcan1) {
                if (motor_group == MOTOR_GROUP_1234_CURRENT)
                    TxHeader_can_M3508.StdId = 0x200;      //电流控制
                for (uint8_t i = 0; i < 3; i++) {                   //发信失败则再试2次
                    if (HAL_OK == HAL_CAN_AddTxMessage(hcan,&TxHeader_can_M3508,can_send_content,&Tx_mail_can_1))
                        break;
                }
                //后四个电机
                if (motor_group == MOTOR_GROUP_5678_CURRENT)
                    TxHeader_can_M3508.StdId = 0x1FF;
                for (uint8_t i = 0; i < 3; i++) {                   //发信失败则再试2次
                    if (HAL_OK == HAL_CAN_AddTxMessage(hcan,&TxHeader_can_M3508,can_send_content,&Tx_mail_can_1))
                        break;
                }
            }
            else if (hcan == &hcan2) {
                if (motor_group == MOTOR_GROUP_1234_CURRENT)
                    TxHeader_can_M3508.StdId = 0x200;      //电流控制
                for (uint8_t i = 0; i < 3; i++) {                   //发信失败则再试2次
                    if (HAL_OK == HAL_CAN_AddTxMessage(hcan,&TxHeader_can_M3508,can_send_content,&Tx_mail_can_2))
                        break;
                }
                //后四个电机
                if (motor_group == MOTOR_GROUP_5678_CURRENT)
                    TxHeader_can_M3508.StdId = 0x1FF;
                for (uint8_t i = 0; i < 3; i++) {                   //发信失败则再试2次
                    if (HAL_OK == HAL_CAN_AddTxMessage(hcan,&TxHeader_can_M3508,can_send_content,&Tx_mail_can_2))
                        break;
                }
            }
            break;

        case MOTOR_TYPE_M2006:

            break;

        default:
            return -1;
    }
    load_status = 0;

    return 0;
}

//------------------------------------------------------------------------------------------------------
// 函数简介		can接收函数
// 使用示例		JJ_motor_can_receive(&hcan1);
// 参数介绍		hcan				目标can总线
// 返回参数		0-正常 -1-异常
// 备注信息       请在中断中定时调用
//------------------------------------------------------------------------------------------------------
int JJ_motor_data_receive(CAN_HandleTypeDef *hcan) {
    uint8_t rx_data[8] = {0,0,0,0,0,0,0,0};
    if(HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0,&RxHeader_can_GM6020, rx_data)== HAL_OK) {
        uint16_t motor_id = 0;
        motor_id = RxHeader_can_GM6020.StdId - 0x204 - 1;
        Motor_Info[motor_id].can_id = RxHeader_can_GM6020.StdId;
        Motor_Info[motor_id].motor_position = (int16_t)((int16_t)rx_data[0]<<8 | (int16_t)rx_data[1]); //最后一次(int16_t)是发现<<和>>运算符会默认类型提升为int
        Motor_Info[motor_id].motor_speed = (int16_t)((int16_t)rx_data[2]<<8 | (int16_t)rx_data[3]);
        Motor_Info[motor_id].torque_current = (int16_t)((int16_t)rx_data[4]<<8 | (int16_t)rx_data[5]);
        Motor_Info[motor_id].temperature = (uint16_t)rx_data[6];
        return 0;
    }
    return -1;
}

//------------------------------------------------------------------------------------------------------
// 函数简介		   单电机-单级-PID位置闭环（限幅和参数请前往pid.c中的pid_init()函数中修改）
// 使用示例         JJ_motor_position_closeCircle(0, &Struct_Pid_GM6020[0].POSITION, MOTOR_1);
// 参数介绍         target_position                         预期角度（位置）,0~8191
// 参数介绍         *hcan                                   目标can总线
// 参数介绍         motor_type                              电机类型
// 参数介绍         motor_group                             电机组和控制类型
// 参数介绍         motor_id                                电机ID，1~4或5~8
// 返回参数         result                                  计算结果
// 备注信息         GM6020（逆时针增大）: 0~8191
// 备注信息         GM6020单位置环最佳参数: (15, 1.5f, 35, Positional, 400, 14000);
//------------------------------------------------------------------------------------------------------
int16_t JJ_motor_position_closeCircle(int target_positon, pidTypedef *struct_pid,const MotorID motor_id) {
    //钳位
    target_positon = clip_int(target_positon, 0, 8191);
    //PID计算
    int diff = target_positon - (int)Motor_Info[motor_id].motor_position;
    if (diff > 4096) diff -= 8192;
    if (diff < -4096) diff += 8192;     //最短路径归中，误差diff超过半圆时立刻将误差diff去相反数，相当于在圆上做 geodesic 误差
    const int16_t result = (int16_t)pid_calculate_error(struct_pid, (pidtype)diff);
    //装载报文
    // JJ_motor_data_load(motor_id, result);
    //根据不同电机类型不同控制类型发报
    // JJ_motor_ctrl(hcan, motor_type, motor_group);
    return result;
}

//------------------------------------------------------------------------------------------------------
// 函数简介		   单电机-单级-PID角度闭环（限幅和参数请前往pid.c中的pid_init()函数中修改）
// 使用示例         JJ_motor_angle_closeCircle(30.0f, &Struct_Pid_GM6020[0].POSITION, MOTOR_1);
// 参数介绍         target_angle                            预期角度（角度）,0~359.99，可小数，精度最低0.5度
// 参数介绍         *hcan                                   目标can总线
// 参数介绍         motor_type                              电机类型
// 参数介绍         motor_group                             电机组和控制类型
// 参数介绍         motor_id                                电机ID，1~4或5~8
// 返回参数         result                                  计算结果
// 备注信息         GM6020（逆时针增大）: 0~359.99     GM6020单角度环最佳参数: (15, 1.5f, 35, Positional, 400, 14000);
// 备注信息
//------------------------------------------------------------------------------------------------------
int16_t JJ_motor_angle_closeCircle(float target_angle, pidTypedef *struct_pid,const MotorID motor_id) {
    //钳位
    target_angle = clip_float(target_angle, 0.0f, 359.99f);
    //角度-位置 换算
    const uint16_t target_position = (uint16_t)(target_angle * (8192.0f / 360.0f));
    //PID计算
    int diff = (int)target_position - (int)Motor_Info[motor_id].motor_position;
    if (diff > 4096) diff -= 8192;
    if (diff < -4096) diff += 8192;     //最短路径归中，误差diff超过半圆时立刻将误差diff去相反数，相当于在圆上做 geodesic 误差
    const int16_t result = (int16_t)pid_calculate_error(struct_pid, (pidtype)diff);
    //装载报文
    // JJ_motor_data_load(motor_id, result);
    //根据不同电机类型不同控制类型发报
    // JJ_motor_ctrl(hcan, motor_type, motor_group);
    return result;
}

//------------------------------------------------------------------------------------------------------
// 函数简介		   单电机-单级-PID速度闭环
// 使用示例         JJ_motor_speed_closeCircle(100, &Struct_Pid_GM6020[0].SPEED, MOTOR_1);
// 参数介绍         target_speed                            预期速度（转速，单位rpm）
// 参数介绍         *hcan                                   目标can总线
// 参数介绍         motor_type                              电机类型
// 参数介绍         motor_group                             电机组和控制类型
// 参数介绍         motor_id                                电机ID，1~4或5~8
// 返回参数         result                                  计算结果
// 备注信息         GM6020（空载）:-320~320rpm        GM6020（额定1.2N·m）: -132~132rpm       GM6020单速度环不错参数: (15, 1.5f, 0, Incremental, 400, 14000);
// 备注信息
// 备注信息
//------------------------------------------------------------------------------------------------------
int16_t JJ_motor_speed_closeCircle(int16_t target_speed, pidTypedef *struct_pid,const MotorID motor_id) {
    //钳位
    target_speed = clip_int16(target_speed, -320, 320);
    //PID计算
    const int16_t result = (int16_t)pid_calculate_tar_cur(struct_pid, target_speed, Motor_Info[motor_id].motor_speed);
    //装载报文缓冲区
    // JJ_motor_data_load(motor_id, result);
    //根据不同电机类型发报
    // JJ_motor_ctrl(hcan, motor_type, motor_group);
    return result;
}

//------------------------------------------------------------------------------------------------------
// 函数简介		   单电机-单级-PID力矩闭环
// 使用示例         JJ_motor_torque_closeCircle(0.2f, &Struct_Pid_GM6020[0].TORQUE, MOTOR_1);
// 参数介绍         target_torque                           预期力矩（单位N·m），可小数，-1200~1200mN·m
// 参数介绍         *hcan                                   目标can总线
// 参数介绍         motor_type                              电机类型
// 参数介绍         motor_group                             电机组和控制类型
// 参数介绍         motor_id                                电机ID，1~4或5~8
// 返回参数         result                                  计算结果
// 备注信息         GM6020（最大连续扭矩）:-1200~1200mN·m        GM6020（最大瞬时扭矩）: -2200~2200mN·m       GM6020单力矩环不错参数:(0.5f, 0.05f, 0, Positional, 800, 10000);
// 备注信息
// 备注信息
//------------------------------------------------------------------------------------------------------
int16_t JJ_motor_torque_closeCircle(float target_torque, pidTypedef *struct_pid,const MotorID motor_id) {
    //钳位
    target_torque = clip_float(target_torque, -1200, 1200); //输出没限幅，但输入限幅了，这可以让他短暂超频瞬时相应变快，但不会持续超过最大连续扭矩
    //力矩-电流换算
    // const float target_current = ((target_torque * 1000.f) / 741.f) * ( 16385.f / 3.f);     //(目标力矩*1000)/741=目标电流
    const float target_current = (target_torque / 741.f) * ( 16385.f / 3.f);     //(目标力矩*1000)/741=目标电流
    //PID计算
    const int16_t result = (int16_t)pid_calculate_tar_cur(struct_pid, target_current, Motor_Info[motor_id].torque_current);
    //装载报文缓冲区
    // JJ_motor_data_load(motor_id, result);
    //根据不同电机类型不同控制类型发报
    // JJ_motor_ctrl(hcan, motor_type, motor_group);
    return result;
}

//------------------------------------------------------------------------------------------------------
// 函数简介		   单电机-两级-PID位置外环-速度内环
// 使用示例         JJ_motor_P_S_closeCircle(monitor_target_position,&Struct_Pid_GM6020[0].POSITION, &Struct_Pid_GM6020[0].SPEED, MOTOR_1);
// 参数介绍         target_position                         预期位置, 0~8191
// 参数介绍         *struct_pid_outside                     外环PID结构体指针
// 参数介绍         *struct_pid_inside                      内环PID结构体指针
// 参数介绍         *hcan                                   目标can总线
// 参数介绍         motor_type                              电机类型
// 参数介绍         motor_group                             电机组和控制类型
// 参数介绍         motor_id                                电机ID，1~4或5~8
// 返回参数         result_inside                           计算结果
// 备注信息         GM6020位置外环不错参数:(0.4f, 0, 7, Positional, 400, 320)       速度内环不错参数:(25, 0.8f, 0, Incremental, 400, 1000)      实际上外环参数还是不对，应该不限幅也能输出跑到320以内才行
// 备注信息
// 备注信息
// 备注信息
//------------------------------------------------------------------------------------------------------
int16_t JJ_motor_P_S_closeCircle(int target_position, pidTypedef *struct_pid_outside, pidTypedef *struct_pid_inside,
                       const MotorID motor_id)
{
    //钳位
    target_position = clip_int(target_position, 0, 8191);
    //PID计算
    const int16_t result_outside = JJ_motor_position_closeCircle(target_position, struct_pid_outside, motor_id);
    const int16_t result_inside = JJ_motor_speed_closeCircle(result_outside, struct_pid_inside, motor_id);
    //装载报文缓冲区
    // JJ_motor_data_load(motor_id, result_inside);
    //根据不同电机类型发报
    // JJ_motor_ctrl(hcan, motor_type, motor_group);
    return result_inside;
}

//------------------------------------------------------------------------------------------------------
// 函数简介		   单电机-两级-PID位置外环-力矩内环
// 使用示例         JJ_motor_P_T_closeCircle(monitor_target_position, &Struct_Pid_GM6020[0].POSITION, &Struct_Pid_GM6020[0].TORQUE, MOTOR_1);
// 参数介绍         target_position                         预期位置, 0~8191
// 参数介绍         *struct_pid_outside                     外环PID结构体指针
// 参数介绍         *struct_pid_inside                      内环PID结构体指针
// 参数介绍         motor_id                                电机ID，1~4或5~8
// 返回参数         result_inside                           计算结果
// 备注信息         GM6020力矩内环不错参数:(0.7f, 0.1f, 0, Incremental, 500, 10000)      位置外环参数(10, 0, 40, Positional, 500, 1200)
// 备注信息
// 备注信息
// 备注信息
//------------------------------------------------------------------------------------------------------
int16_t JJ_motor_P_T_closeCircle(int target_position, pidTypedef *struct_pid_outside, pidTypedef *struct_pid_inside,
                       const MotorID motor_id)
{
    //钳位
    target_position = clip_int(target_position, 0, 8191);
    //PID计算
    const int16_t result_outside = JJ_motor_position_closeCircle(target_position, struct_pid_outside, motor_id);
    const int16_t result_inside = JJ_motor_torque_closeCircle(result_outside, struct_pid_inside, motor_id);
    //装载报文缓冲区
    // JJ_motor_data_load(motor_id, result_inside);
    //根据不同电机类型发报
    // JJ_motor_ctrl(hcan, motor_type, motor_group);
    return result_inside;
}

//------------------------------------------------------------------------------------------------------
// 函数简介		   单电机-两级-PID速度外环-力矩内环
// 使用示例         JJ_motor_P_T_closeCircle(monitor_target_position, &Struct_Pid_GM6020[0].SPEED, &Struct_Pid_GM6020[0].TORQUE, MOTOR_1);
// 参数介绍         target_position                         预期位置, 0~8191
// 参数介绍         *struct_pid_outside                     外环PID结构体指针
// 参数介绍         *struct_pid_inside                      内环PID结构体指针
// 参数介绍         motor_id                                电机ID，1~4或5~8
// 返回参数         result_inside                           计算结果
// 备注信息         GM6020速度外环不错参数:(20, 1, 5, Incremental, 500, 1200)       力矩内环不错参数:(0.5f, 0.1f, 0, Incremental, 500, 10000)
// 备注信息
// 备注信息
// 备注信息
//------------------------------------------------------------------------------------------------------
int16_t JJ_motor_S_T_closeCircle(int16_t target_speed, pidTypedef *struct_pid_outside, pidTypedef *struct_pid_inside,
                                 const MotorID motor_id)
{
    //钳位
    target_speed = clip_int16(target_speed, -320, 320);
    //PID计算
    const int16_t result_outside = JJ_motor_speed_closeCircle(target_speed, struct_pid_outside, motor_id);
    const int16_t result_inside = JJ_motor_torque_closeCircle(result_outside, struct_pid_inside, motor_id);
    //装载报文缓冲区
    // JJ_motor_data_load(motor_id, result_inside);
    //根据不同电机类型发报
    // JJ_motor_ctrl(hcan, motor_type, motor_group);
    return result_inside;
}

//------------------------------------------------------------------------------------------------------
// 函数简介         单电机-MIT力矩控制(不宜震荡且非常简单)
// 使用示例         JJ_motor_MIT_closeCircle(500, monitor_target_position, 0, MOTOR_1);
// 参数介绍         target_torque_frontFeed                 前馈力矩（单位N·m），可小数，-1200~1200mN·m
// 参数介绍         target_positon                          目标位置（单位：角度），0~8191
// 参数介绍         target_speed                            目标速度, MIT协议下取0
// 参数介绍         *hcan                                   目标can总线
// 参数介绍         motor_type                              电机类型
// 参数介绍         motor_group                             电机组和控制类型
// 参数介绍         motor_id                                电机ID，1~4或5~8
// 返回参数         result                                  计算结果
// 备注信息         GM6020（最大连续扭矩）:-1200~1200mN·m       位置KP用4      速度KP用40
// 备注信息         MIT公式：output = torque_frontFeed + Kp * (target_position - current_position) + Kd * (target_speed - current_speed)
// 备注信息
//------------------------------------------------------------------------------------------------------
int16_t JJ_motor_MIT_closeCircle(float target_torque_frontFeed, int target_position, int16_t target_speed,
                       const MotorID motor_id) {
    //钳位
    target_torque_frontFeed = clip_float(target_torque_frontFeed, -1200.f, 1200.f);
    target_position = clip_int(target_position, 0, 8191);
    target_speed = clip_int16(target_speed, -320, 320);
    //力矩-电流换算
    const float target_current_frontfeed = (target_torque_frontFeed  / 741.f) * ( 16385.f / 3.f);
    //位置信息处理
    const int16_t diff_p = JJ_motor_position_closeCircle(target_position, &Struct_Pid_GM6020[motor_id].MIT.POSITION, motor_id);
    //速度阻尼处理
    const int16_t diff_s = JJ_motor_speed_closeCircle(target_speed, &Struct_Pid_GM6020[motor_id].MIT.SPEED, motor_id);
    //PID计算
    const int16_t result = (int16_t)((int16_t)target_current_frontfeed + diff_p + diff_s);
    //装载报文缓冲区
    // JJ_motor_data_load(motor_id, result);
    //根据不同电机类型发报
    // JJ_motor_ctrl(hcan, motor_type, motor_group);
    return result;
}

//------------------------------------------------------------------------------------------------------
// 函数简介         遥控器数值换算
// 参数介绍         motor_type                              电机类型
// 参数介绍         solve_mode                              遥控器解算目标
// 使用示例
// 返回参数
// 备注信息
//------------------------------------------------------------------------------------------------------
// void JJ_motor_dbus_solve(MotorType motor_type, Dbus_SolveMode solve_mode) {
//     switch (motor_type) {
//
//         case MOTOR_TYPE_GM6020:
//             switch (solve_mode) {
//                 case TORQUE:
//                     break;
//                 case POSITION:
//                     break;
//                 case SPEED:
//                     break;
//                 case CURRENT:
//                     break;
//                 case VOLTAGE:
//                     break;
//             }
//             break;
//
//         default:
//             break;
//     }
// }

// 下面的是废弃函数别看!!!
// //------------------------------------------------------------------------------------------------------
// // 函数简介		   4电机-单级-PID位置闭环（限幅和参数请前往pid.c中的pid_init()函数中修改）
// // 使用示例         JJ_motor_x4_position_closeCircle(0, 0, 0, 0, &Struct_Pid_GM6020_1, &Struct_Pid_GM6020_2, &Struct_Pid_GM6020_3, &Struct_Pid_GM6020_4, &hcan1, MOTOR_TYPE_GM6020, MOTOR_GROUP_1234_CURRENT);
// // 参数介绍         target_position_x                       电机x的预期位置,0~8191
// // 参数介绍         *struct_pid_1                           电机xPID参数结构体指针
// // 参数介绍         *hcan                                   目标can总线
// // 参数介绍         motor_type                              电机类型
// // 参数介绍         motor_group                             电机组和控制类型
// // 返回参数         void
// // 备注信息         GM6020（逆时针增大）: 0~8191       GM6020单位置环最佳参数: (15, 1.5f, 35, Positional, 400, 14000);
// // 备注信息
// //------------------------------------------------------------------------------------------------------
// void JJ_motor_x4_position_closeCircle(int target_positon_1, int target_positon_2, int target_positon_3, int target_positon_4,
//                                pidTypedef *struct_pid_1, pidTypedef *struct_pid_2, pidTypedef *struct_pid_3,
//                                pidTypedef *struct_pid_4,
//                                CAN_HandleTypeDef *hcan, const MotorType motor_type,
//                                const MotorGroup motor_group)
// {
//     //钳位
//     target_positon_1 = clip_int(target_positon_1, 0, 8191);
//     target_positon_2 = clip_int(target_positon_2, 0, 8191);
//     target_positon_3 = clip_int(target_positon_3, 0, 8191);
//     target_positon_4 = clip_int(target_positon_4, 0, 8191);
//     //误差计算
//     int diff[4] = {0};
//     if (motor_group == MOTOR_GROUP_1234_VOLTAGE || motor_group == MOTOR_GROUP_1234_CURRENT) {
//         diff[0] = target_positon_1 - (int)Motor_Info[0].motor_angle;
//         diff[1] = target_positon_2 - (int)Motor_Info[1].motor_angle;
//         diff[2] = target_positon_3 - (int)Motor_Info[2].motor_angle;
//         diff[3] = target_positon_4 - (int)Motor_Info[3].motor_angle;
//     }
//     else if (motor_group == MOTOR_GROUP_5678_VOLTAGE || motor_group == MOTOR_GROUP_5678_CURRENT) {
//         diff[0] = target_positon_1 - (int)Motor_Info[4].motor_angle;
//         diff[1] = target_positon_2 - (int)Motor_Info[5].motor_angle;
//         diff[2] = target_positon_3 - (int)Motor_Info[6].motor_angle;
//         diff[3] = target_positon_4 - (int)Motor_Info[7].motor_angle;
//     }
//     for (int i = 0; i < 4; i++) {
//         if (diff[i] > 4096) diff[i] -= 8192;
//         if (diff[i] < -4096) diff[i] += 8192;     //最短路径归中，误差diff超过半圆时立刻将误差diff去相反数，相当于在圆上做 geodesic 误差
//     }
//     //PID计算
//     int16_t result[4] = {0};
//     result[0] = (int16_t)pid_calculate_error(struct_pid_1, (pidtype)diff[0]);
//     result[1] = (int16_t)pid_calculate_error(struct_pid_2, (pidtype)diff[1]);
//     result[2] = (int16_t)pid_calculate_error(struct_pid_3, (pidtype)diff[2]);
//     result[3] = (int16_t)pid_calculate_error(struct_pid_4, (pidtype)diff[3]);
//     //装载报文
//     JJ_motor_data_load(MOTOR_1, result[0]);
//     JJ_motor_data_load(MOTOR_2, result[1]);
//     JJ_motor_data_load(MOTOR_3, result[2]);
//     JJ_motor_data_load(MOTOR_4, result[3]);
//     //根据不同电机类型不同控制类型发报
//     // JJ_motor_ctrl(hcan, motor_type, motor_group);
// }
//
// //------------------------------------------------------------------------------------------------------
// // 函数简介		   4电机-单级-PID角度闭环（限幅和参数请前往pid.c中的pid_init()函数中修改）
// // 使用示例         JJ_motor_x4_angle_closeCircle(0, 0, 0, 0, &Struct_Pid_GM6020_1, &Struct_Pid_GM6020_2, &Struct_Pid_GM6020_3, &Struct_Pid_GM6020_4, &hcan1, MOTOR_TYPE_GM6020, MOTOR_GROUP_1234_CURRENT);
// // 参数介绍         target_angle_x                          电机x的预期角度,0~359.99
// // 参数介绍         *struct_pid_x                           电机xPID参数结构体指针
// // 参数介绍         *hcan                                   目标can总线
// // 参数介绍         motor_type                              电机类型
// // 参数介绍         motor_group                             电机组和控制类型
// // 返回参数         void
// // 备注信息         GM6020（逆时针增大）: 0~359.99     GM6020单角度环最佳参数: (15, 1.5f, 35, Positional, 400, 14000);
// // 备注信息
// //------------------------------------------------------------------------------------------------------
// void JJ_motor_x4_angle_closeCircle(float target_angle_1, float target_angle_2, float target_angle_3, float target_angle_4,
//                                pidTypedef *struct_pid_1, pidTypedef *struct_pid_2, pidTypedef *struct_pid_3,
//                                pidTypedef *struct_pid_4,
//                                CAN_HandleTypeDef *hcan, const MotorType motor_type,
//                                const MotorGroup motor_group)
// {
//     //钳位
//     target_angle_1 = clip_float(target_angle_1, 0, 359.99f);
//     target_angle_2 = clip_float(target_angle_2, 0, 359.99f);
//     target_angle_3 = clip_float(target_angle_3, 0, 359.99f);
//     target_angle_4 = clip_float(target_angle_4, 0, 359.99f);
//     //角度-位置 换算
//     const uint16_t target_position_1 = (uint16_t)(target_angle_1 * (8192.0f / 360.0f));
//     const uint16_t target_position_2 = (uint16_t)(target_angle_2 * (8192.0f / 360.0f));
//     const uint16_t target_position_3 = (uint16_t)(target_angle_3 * (8192.0f / 360.0f));
//     const uint16_t target_position_4 = (uint16_t)(target_angle_4 * (8192.0f / 360.0f));
//     //误差计算
//     int diff[4] = {0};
//     if (motor_group == MOTOR_GROUP_1234_VOLTAGE || motor_group == MOTOR_GROUP_1234_CURRENT) {
//         diff[0] = target_position_1 - (int)Motor_Info[0].motor_angle;
//         diff[1] = target_position_2 - (int)Motor_Info[1].motor_angle;
//         diff[2] = target_position_3 - (int)Motor_Info[2].motor_angle;
//         diff[3] = target_position_4 - (int)Motor_Info[3].motor_angle;
//     }
//     else if (motor_group == MOTOR_GROUP_5678_VOLTAGE || motor_group == MOTOR_GROUP_5678_CURRENT) {
//         diff[0] = target_position_1 - (int)Motor_Info[4].motor_angle;
//         diff[1] = target_position_2 - (int)Motor_Info[5].motor_angle;
//         diff[2] = target_position_3 - (int)Motor_Info[6].motor_angle;
//         diff[3] = target_position_4 - (int)Motor_Info[7].motor_angle;
//     }
//     for (int i = 0; i < 4; i++) {
//         if (diff[i] > 4096) diff[i] -= 8192;
//         if (diff[i] < -4096) diff[i] += 8192;     //最短路径归中，误差diff超过半圆时立刻将误差diff去相反数，相当于在圆上做 geodesic 误差
//     }
//     //PID计算
//     int16_t result[4] = {0};
//     result[0] = (int16_t)pid_calculate_error(struct_pid_1, (pidtype)diff[0]);
//     result[1] = (int16_t)pid_calculate_error(struct_pid_2, (pidtype)diff[1]);
//     result[2] = (int16_t)pid_calculate_error(struct_pid_3, (pidtype)diff[2]);
//     result[3] = (int16_t)pid_calculate_error(struct_pid_4, (pidtype)diff[3]);
//     //装载报文
//     JJ_motor_data_load(MOTOR_1, result[0]);
//     JJ_motor_data_load(MOTOR_2, result[1]);
//     JJ_motor_data_load(MOTOR_3, result[2]);
//     JJ_motor_data_load(MOTOR_4, result[3]);
//     //根据不同电机类型不同控制类型发报
//     // JJ_motor_ctrl(hcan, motor_type, motor_group);
// }
//
// //------------------------------------------------------------------------------------------------------
// // 函数简介		   4电机-单级-PID速度闭环（限幅和参数请前往pid.c中的pid_init()函数中修改）
// // 使用示例         JJ_motor_x4_speed_closeCircle(50, 0, -50, 0, &Struct_Pid_GM6020_1, &Struct_Pid_GM6020_2, &Struct_Pid_GM6020_3, &Struct_Pid_GM6020_4, &hcan1, MOTOR_TYPE_GM6020, MOTOR_GROUP_1234_CURRENT);
// // 参数介绍         target_speed_x                          电机x 的预期速度
// // 参数介绍         *struct_pid_x                           电机x PID参数结构体指针
// // 参数介绍         *hcan                                   目标can总线
// // 参数介绍         motor_type                              电机类型
// // 参数介绍         motor_group                             电机组和控制类型
// // 返回参数         void
// // 备注信息         GM6020（逆时针增大）: -320~320rpm     GM6020单速度环不错参数: (15, 1.5f, 0, Incremental, 400, 14000);
// // 备注信息
// //------------------------------------------------------------------------------------------------------
// void JJ_motor_x4_speed_closeCircle(int16_t target_speed_1, int16_t target_speed_2, int16_t target_speed_3, int16_t target_speed_4,
//                                pidTypedef *struct_pid_1, pidTypedef *struct_pid_2, pidTypedef *struct_pid_3,
//                                pidTypedef *struct_pid_4,
//                                CAN_HandleTypeDef *hcan, const MotorType motor_type,
//                                const MotorGroup motor_group)
// {
//     //钳位
//     target_speed_1 = clip_int16(target_speed_1, -320, 320);
//     target_speed_2 = clip_int16(target_speed_2, -320, 320);
//     target_speed_3 = clip_int16(target_speed_3, -320, 320);
//     target_speed_4 = clip_int16(target_speed_4, -320, 320);
//     //误差计算
//     int16_t diff[4] = {0};
//     if (motor_group == MOTOR_GROUP_1234_VOLTAGE || motor_group == MOTOR_GROUP_1234_CURRENT) {
//         diff[0] = (int16_t)(target_speed_1 - Motor_Info[0].motor_speed);
//         diff[1] = (int16_t)(target_speed_2 - Motor_Info[1].motor_speed);
//         diff[2] = (int16_t)(target_speed_3 - Motor_Info[2].motor_speed);
//         diff[3] = (int16_t)(target_speed_4 - Motor_Info[3].motor_speed);
//     }
//     else if (motor_group == MOTOR_GROUP_5678_VOLTAGE || motor_group == MOTOR_GROUP_5678_CURRENT) {
//         diff[0] = (int16_t)(target_speed_1 - Motor_Info[4].motor_speed);
//         diff[1] = (int16_t)(target_speed_2 - Motor_Info[5].motor_speed);
//         diff[2] = (int16_t)(target_speed_3 - Motor_Info[6].motor_speed);
//         diff[3] = (int16_t)(target_speed_4 - Motor_Info[7].motor_speed);
//     }
//     //PID计算
//     int16_t result[4] = {0};
//     result[0] = (int16_t)pid_calculate_error(struct_pid_1, (pidtype)diff[0]);
//     result[1] = (int16_t)pid_calculate_error(struct_pid_2, (pidtype)diff[1]);
//     result[2] = (int16_t)pid_calculate_error(struct_pid_3, (pidtype)diff[2]);
//     result[3] = (int16_t)pid_calculate_error(struct_pid_4, (pidtype)diff[3]);
//     //装载报文
//     JJ_motor_data_load(MOTOR_1, result[0]);
//     JJ_motor_data_load(MOTOR_2, result[1]);
//     JJ_motor_data_load(MOTOR_3, result[2]);
//     JJ_motor_data_load(MOTOR_4, result[3]);
//     //根据不同电机类型不同控制类型发报
//     // JJ_motor_ctrl(hcan, motor_type, motor_group);
// }
//
// //------------------------------------------------------------------------------------------------------
// // 函数简介		   4电机-单级-PID力矩闭环（限幅和参数请前往pid.c中的pid_init()函数中修改）
// // 使用示例         JJ_motor_x4_torque_closeCircle(0.05f, 0, -0.3f, 0, &Struct_Pid_GM6020_1, &Struct_Pid_GM6020_2, &Struct_Pid_GM6020_3, &Struct_Pid_GM6020_4,
// //                                         &hcan1, MOTOR_TYPE_GM6020, MOTOR_GROUP_1234_CURRENT);
// // 参数介绍         target_torque_x                         电机x 的预期力矩
// // 参数介绍         *struct_pid_x                           电机x PID参数结构体指针
// // 参数介绍         *hcan                                   目标can总线
// // 参数介绍         motor_type                              电机类型
// // 参数介绍         motor_group                             电机组和控制类型
// // 返回参数         void
// // 备注信息         GM6020（最大连续扭矩）:-1200~1200mN·m         GM6020（最大瞬时扭矩）: -2200~2200mN·m       GM6020单力矩环不错参数:(0.5f, 0.05f, 0, Positional, 800, 10000);
// // 备注信息
// //------------------------------------------------------------------------------------------------------
// void JJ_motor_x4_torque_closeCircle(float target_torque_1, float target_torque_2, float target_torque_3, float target_torque_4,
//                                pidTypedef *struct_pid_1, pidTypedef *struct_pid_2, pidTypedef *struct_pid_3,
//                                pidTypedef *struct_pid_4,
//                                CAN_HandleTypeDef *hcan, const MotorType motor_type,
//                                const MotorGroup motor_group)
// {
//     //钳位
//     target_torque_1 = clip_float(target_torque_1, -1200, 1200);
//     target_torque_2 = clip_float(target_torque_2, -1200, 1200);
//     target_torque_3 = clip_float(target_torque_3, -1200, 1200);
//     target_torque_4 = clip_float(target_torque_4, -1200, 1200);
//     //力矩-电流换算
//     const float target_current_1 = (target_torque_1 / 741.f) * ( 16385.f / 3.f);
//     const float target_current_2 = (target_torque_2 / 741.f) * ( 16385.f / 3.f);
//     const float target_current_3 = (target_torque_3 / 741.f) * ( 16385.f / 3.f);
//     const float target_current_4 = (target_torque_4 / 741.f) * ( 16385.f / 3.f);
//     //误差计算
//     int16_t diff[4] = {0};
//     if (motor_group == MOTOR_GROUP_1234_VOLTAGE || motor_group == MOTOR_GROUP_1234_CURRENT) {
//         diff[0] = (int16_t)(target_current_1 - (float)Motor_Info[0].torque_current);
//         diff[1] = (int16_t)(target_current_2 - (float)Motor_Info[1].torque_current);
//         diff[2] = (int16_t)(target_current_3 - (float)Motor_Info[2].torque_current);
//         diff[3] = (int16_t)(target_current_4 - (float)Motor_Info[3].torque_current);
//     }
//     else if (motor_group == MOTOR_GROUP_5678_VOLTAGE || motor_group == MOTOR_GROUP_5678_CURRENT) {
//         diff[0] = (int16_t)(target_current_1 - (float)Motor_Info[4].torque_current);
//         diff[1] = (int16_t)(target_current_2 - (float)Motor_Info[5].torque_current);
//         diff[2] = (int16_t)(target_current_3 - (float)Motor_Info[6].torque_current);
//         diff[3] = (int16_t)(target_current_4 - (float)Motor_Info[7].torque_current);
//     }
//     //PID计算
//     int16_t result[4] = {0};
//     result[0] = (int16_t)pid_calculate_error(struct_pid_1, (pidtype)diff[0]);
//     result[1] = (int16_t)pid_calculate_error(struct_pid_2, (pidtype)diff[1]);
//     result[2] = (int16_t)pid_calculate_error(struct_pid_3, (pidtype)diff[2]);
//     result[3] = (int16_t)pid_calculate_error(struct_pid_4, (pidtype)diff[3]);
//     //装载报文
//     JJ_motor_data_load(MOTOR_1, result[0]);
//     JJ_motor_data_load(MOTOR_2, result[1]);
//     JJ_motor_data_load(MOTOR_3, result[2]);
//     JJ_motor_data_load(MOTOR_4, result[3]);
//     //根据不同电机类型不同控制类型发报
//     // JJ_motor_ctrl(hcan, motor_type, motor_group);
// }
//
// //------------------------------------------------------------------------------------------------------
// // 函数简介         4电机-两级-PID位置外环-速度内环（非常夸张，要传入8套PID参数）
// // 使用示例         JJ_motor_x4_P_S_closeCircle(0, 0, 0, 0, &Struct_Pid_GM6020_1_POSITION, &Struct_Pid_GM6020_2_POSITION,
// //                                      &Struct_Pid_GM6020_3_POSITION, &Struct_Pid_GM6020_4_POSITION, &Struct_Pid_GM6020_1_SPEED,
// //                                      &Struct_Pid_GM6020_2_SPEED, &Struct_Pid_GM6020_3_SPEED, &Struct_Pid_GM6020_4_SPEED, &hcan1,
// //                                      MOTOR_TYPE_GM6020, MOTOR_GROUP_1234_CURRENT);
// // 参数介绍         target_position_x                       电机x 的预期位置,0~8191
// // 参数介绍         *struct_pid_outside_x                   电机x 外环PID结构体指针
// // 参数介绍         *struct_pid_inside_x                    电机x 内环PID结构体指针
// // 参数介绍         *hcan                                   目标can总线
// // 参数介绍         motor_type                              电机类型
// // 参数介绍         motor_group                             电机组和控制类型
// // 参数介绍         motor_id                                电机ID，1~4或5~8
// // 返回参数         void
// // 备注信息         GM6020位置外环不错参数:(0.4f, 0, 7, Positional, 400, 320)       速度内环不错参数:(25（空载19）, 0.8f, 0, Incremental, 400, 1000)
// // 备注信息
// // 备注信息
// //------------------------------------------------------------------------------------------------------
// void JJ_motor_x4_P_S_closeCircle(int target_position_1, int target_position_2, int target_position_3, int target_position_4,
//                        pidTypedef *struct_pid_outside_1, pidTypedef *struct_pid_outside_2,
//                        pidTypedef *struct_pid_outside_3, pidTypedef *struct_pid_outside_4,
//                        pidTypedef *struct_pid_inside_1, pidTypedef *struct_pid_inside_2,
//                        pidTypedef *struct_pid_inside_3, pidTypedef *struct_pid_inside_4,
//                        CAN_HandleTypeDef *hcan, const MotorType motor_type, const MotorGroup motor_group)
// {
//     //钳位
//     target_position_1 = clip_int(target_position_1, 0, 8191);
//     target_position_2 = clip_int(target_position_2, 0, 8191);
//     target_position_3 = clip_int(target_position_3, 0, 8191);
//     target_position_4 = clip_int(target_position_4, 0, 8191);
//     //误差计算
//     int diff[4] = {0};
//     if (motor_group == MOTOR_GROUP_1234_VOLTAGE || motor_group == MOTOR_GROUP_1234_CURRENT) {
//         diff[0] = target_position_1 - (int)Motor_Info[0].motor_angle;
//         diff[1] = target_position_2 - (int)Motor_Info[1].motor_angle;
//         diff[2] = target_position_3 - (int)Motor_Info[2].motor_angle;
//         diff[3] = target_position_4 - (int)Motor_Info[3].motor_angle;
//     }
//     else if (motor_group == MOTOR_GROUP_5678_VOLTAGE || motor_group == MOTOR_GROUP_5678_CURRENT) {
//         diff[0] = target_position_1 - (int)Motor_Info[4].motor_angle;
//         diff[1] = target_position_2 - (int)Motor_Info[5].motor_angle;
//         diff[2] = target_position_3 - (int)Motor_Info[6].motor_angle;
//         diff[3] = target_position_4 - (int)Motor_Info[7].motor_angle;
//     }
//     for (int i = 0; i < 4; i++) {
//         if (diff[i] > 4096) diff[i] -= 8192;
//         if (diff[i] < -4096) diff[i] += 8192;     //最短路径归中，误差diff超过半圆时立刻将误差diff去相反数，相当于在圆上做 geodesic 误差
//     }
//     //PID计算
//     int16_t result_outside[4] = {0};    //外环
//     result_outside[0] = (int16_t)pid_calculate_error(struct_pid_outside_1, (pidtype)diff[0]);
//     result_outside[1] = (int16_t)pid_calculate_error(struct_pid_outside_2, (pidtype)diff[1]);
//     result_outside[2] = (int16_t)pid_calculate_error(struct_pid_outside_3, (pidtype)diff[2]);
//     result_outside[3] = (int16_t)pid_calculate_error(struct_pid_outside_4, (pidtype)diff[3]);
//     int16_t result_inside[4] = {0};     //内环
//     result_inside[0] = (int16_t)pid_calculate_error(struct_pid_inside_1, (pidtype)result_outside[0]);
//     result_inside[1] = (int16_t)pid_calculate_error(struct_pid_inside_2, (pidtype)result_outside[1]);
//     result_inside[2] = (int16_t)pid_calculate_error(struct_pid_inside_3, (pidtype)result_outside[2]);
//     result_inside[3] = (int16_t)pid_calculate_error(struct_pid_inside_4, (pidtype)result_outside[3]);
//     //装载报文缓冲区
//     JJ_motor_data_load(MOTOR_1, result_inside[0]);
//     JJ_motor_data_load(MOTOR_2, result_inside[1]);
//     JJ_motor_data_load(MOTOR_3, result_inside[2]);
//     JJ_motor_data_load(MOTOR_4, result_inside[3]);
//     //根据不同电机类型发报
//     // JJ_motor_ctrl(hcan, motor_type, motor_group);
// }

//------------------------------------------------------------------------------------------------------
// 函数简介         4电机-MIT力矩控制
// 使用示例         JJ_motor_x4_MIT_closeCircle(0.3f, 0, 0, 0, monitor_target_position,
//                                             monitor_target_position, monitor_target_position,monitor_target_position, 0, 0, 0, 0,
//                                             &hcan1, MOTOR_TYPE_GM6020, MOTOR_GROUP_1234_CURRENT);
// 参数介绍         target_torque_frontFeed_x               前馈力矩（单位N·m），可小数，-1.2~1.2N·m
// 参数介绍         target_positon_X                        目标位置（单位：角度），0~8191
// 参数介绍         target_speed_x                          目标速度（单位：RPM），-320~320
// 参数介绍         *hcan                                   目标can总线
// 参数介绍         motor_type                              电机类型
// 参数介绍         motor_group                             电机组和控制类型
// 返回参数         void
// 备注信息         GM6020（最大连续扭矩）:-1.2~1.2N·m
// 备注信息         MIT公式：output = torque_frontFeed + Kp * (target_position - current_position) + Kd * (target_speed - current_speed)
// 备注信息         注意这里受到MIT控制的电机共享同一套参数
//------------------------------------------------------------------------------------------------------
// void JJ_motor_x4_MIT_closeCircle(float target_torque_frontFeed_1, float target_torque_frontFeed_2, float target_torque_frontFeed_3, float target_torque_frontFeed_4,
//                        int target_position_1, int target_position_2, int target_position_3, int target_position_4,
//                        int16_t target_speed_1, int16_t target_speed_2, int16_t target_speed_3, int16_t target_speed_4,
//                        CAN_HandleTypeDef *hcan, const MotorType motor_type, const MotorGroup motor_group) {
//     //钳位
//     target_torque_frontFeed_1 = clip_float(target_torque_frontFeed_1, -1.2f, 1.2f);
//     target_torque_frontFeed_2 = clip_float(target_torque_frontFeed_2, -1.2f, 1.2f);
//     target_torque_frontFeed_3 = clip_float(target_torque_frontFeed_3, -1.2f, 1.2f);
//     target_torque_frontFeed_4 = clip_float(target_torque_frontFeed_4, -1.2f, 1.2f);
//     target_position_1 = clip_int(target_position_1, 0, 8191);
//     target_position_2 = clip_int(target_position_2, 0, 8191);
//     target_position_3 = clip_int(target_position_3, 0, 8191);
//     target_position_4 = clip_int(target_position_4, 0, 8191);
//     target_speed_1 = clip_int16(target_speed_1, -320, 320);
//     target_speed_2 = clip_int16(target_speed_2, -320, 320);
//     target_speed_3 = clip_int16(target_speed_3, -320, 320);
//     target_speed_4 = clip_int16(target_speed_4, -320, 320);
//     //力矩-电流换算
//     const float target_frontfeed[4] = {((target_torque_frontFeed_1 * 1000.f) / 741.f) * ( 16385.f / 3.f),
//                                             ((target_torque_frontFeed_2 * 1000.f) / 741.f) * ( 16385.f / 3.f),
//                                             ((target_torque_frontFeed_3 * 1000.f) / 741.f) * ( 16385.f / 3.f),
//                                             ((target_torque_frontFeed_4 * 1000.f) / 741.f) * ( 16385.f / 3.f)};
//     //位置信息处理
//     int diff_p[4] = {0};
//     if (motor_group == MOTOR_GROUP_1234_VOLTAGE || motor_group == MOTOR_GROUP_1234_CURRENT) {
//         diff_p[0] = target_position_1 - (int)Motor_Info[0].motor_angle;
//         diff_p[1] = target_position_2 - (int)Motor_Info[1].motor_angle;
//         diff_p[2] = target_position_3 - (int)Motor_Info[2].motor_angle;
//         diff_p[3] = target_position_4 - (int)Motor_Info[3].motor_angle;
//     }
//     else if (motor_group == MOTOR_GROUP_5678_VOLTAGE || motor_group == MOTOR_GROUP_5678_CURRENT) {
//         diff_p[0] = target_position_1 - (int)Motor_Info[4].motor_angle;
//         diff_p[1] = target_position_2 - (int)Motor_Info[5].motor_angle;
//         diff_p[2] = target_position_3 - (int)Motor_Info[6].motor_angle;
//         diff_p[3] = target_position_4 - (int)Motor_Info[7].motor_angle;
//     }
//     for (int i = 0; i < 4; i++) {
//         if (diff_p[i] > 4096) diff_p[i] -= 8192;
//         if (diff_p[i] < -4096) diff_p[i] += 8192;     //最短路径归中，误差diff超过半圆时立刻将误差diff去相反数，相当于在圆上做 geodesic 误差
//     }
//     //速度误差计算
//     int16_t diff_s[4] = {0};
//     if (motor_group == MOTOR_GROUP_1234_VOLTAGE || motor_group == MOTOR_GROUP_1234_CURRENT) {
//         diff_s[0] = (int16_t)(target_speed_1 - Motor_Info[0].motor_speed);
//         diff_s[1] = (int16_t)(target_speed_2 - Motor_Info[1].motor_speed);
//         diff_s[2] = (int16_t)(target_speed_3 - Motor_Info[2].motor_speed);
//         diff_s[3] = (int16_t)(target_speed_4 - Motor_Info[3].motor_speed);
//     }
//     else if (motor_group == MOTOR_GROUP_5678_VOLTAGE || motor_group == MOTOR_GROUP_5678_CURRENT) {
//         diff_s[0] = (int16_t)(target_speed_1 - Motor_Info[4].motor_speed);
//         diff_s[1] = (int16_t)(target_speed_2 - Motor_Info[5].motor_speed);
//         diff_s[2] = (int16_t)(target_speed_3 - Motor_Info[6].motor_speed);
//         diff_s[3] = (int16_t)(target_speed_4 - Motor_Info[7].motor_speed);
//     }
//     //PID计算（注意这里受到MIT控制的电机共享同一套参数）
//     int16_t result[4] = {0};
//     for (int i = 0; i < 4; i++) {
//         result[i] = (int16_t)((int16_t)target_frontfeed[i]
//                     + (int16_t)pid_calculate_error(&Struct_Pid_GM6020_MIT_POSITION, (pidtype)diff_p[i])
//                     + (int16_t)pid_calculate_error(&Struct_Pid_GM6020_MIT_SPEED, diff_s[i]));
//     }
//     //装载报文缓冲区
//     JJ_motor_data_load(MOTOR_1, result[0]);
//     JJ_motor_data_load(MOTOR_2, result[1]);
//     JJ_motor_data_load(MOTOR_3, result[2]);
//     JJ_motor_data_load(MOTOR_4, result[3]);
//     //根据不同电机类型发报
//     // JJ_motor_ctrl(hcan, motor_type, motor_group);
// }


